/**********************************************************************
 * file:  sr_router.c 
 * date:  Mon Feb 18 12:50:42 PST 2002  
 * Contact: casado@stanford.edu 
 *
 * Description:
 * 
 * This file contains all the functions that interact directly
 * with the routing table, as well as the main entry method
 * for routing.
 *
 **********************************************************************/

#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>


#include "sr_if.h"
#include "sr_rt.h"
#include "sr_rt.h"
#include "if_tt.h"


#include "sr_router.h"
#include "sr_protocol.h"
#include "p_queue.h"

#define  DEBUG_PACKET   0
#define  DEBUG_PRINT    0

/*--------------------------------------------------------------------- 
 * Method: sr_init(void)
 * Scope:  Global
 *
 * Initialize the routing subsystem
 * 
 *---------------------------------------------------------------------*/

void sr_init(struct sr_instance* sr) 
{
    /* REQUIRES */
    assert(sr);

    /* Add initialization code here! */

} /* -- sr_init -- */

void print_packet(uint8_t * buf, int len)
{
    int i=0;
    printf("Packet Data:");
    for (i=0; i<len; i++)
    {
        if (i%16 == 0) printf("\n\t\t");
        printf("%x ", buf[i]);
    }
    printf("\n");
}


/*---------------------------------------------------------------------
 * Method: sr_handlepacket(uint8_t* p,char* interface)
 * Scope:  Global
 *
 * This method is called each time the router receives a packet on the
 * interface.  The packet buffer, the packet length and the receiving
 * interface are passed in as parameters. The packet is complete with
 * ethernet headers.
 *
 * Note: Both the packet buffer and the character's memory are handled
 * by sr_vns_comm.c that means do NOT delete either.  Make a copy of the
 * packet instead if you intend to keep it around beyond the scope of
 * the method call.
 *
 *---------------------------------------------------------------------*/

void sr_handlepacket(struct sr_instance* sr, 
        uint8_t * packet/* lent */,
        unsigned int len,
        char* interface/* lent */)
{
    struct sr_if*           iface = 0;
    struct sr_ethernet_hdr* e_hdr = 0;
    struct sr_arphdr*       a_hdr = 0;
    struct if_tt*           arp_table = 0;
    
    /* REQUIRES */
    assert(sr);
    assert(packet);
    assert(interface);

    if (DEBUG_PRINT)
        printf("*** -> Received packet of length %d via %s\n",len, interface);
 
    iface = sr_get_interface(sr, interface);
    assert(iface);    

    e_hdr = (struct sr_ethernet_hdr*)packet;
    a_hdr = (struct sr_arphdr*)(packet + sizeof(struct sr_ethernet_hdr));

    /* check timeouts on arp table entries and discard expired*/
    if_check_time(iface);
    
    if (DEBUG_PRINT)
        sr_print_if_tables(sr);

    /*now we check the packet contents and act accordingly*/
    switch(ntohs(e_hdr->ether_type))
    {
        case ETHERTYPE_ARP:
            if (DEBUG_PRINT)
                printf("\tARP");
            
            a_hdr = (struct sr_arphdr*)(packet + sizeof(struct sr_ethernet_hdr));

            /*
             * Check if whe have this packets sender info in our arp table
             * if we have it, update it
             * if not, add it
             */
            if((arp_table = if_find_trans(iface, (uint32_t*)&a_hdr->ar_sip)) == 0)
            {
                if_add_trans(iface, (unsigned char*)&a_hdr->ar_sha, (uint32_t*)&a_hdr->ar_sip);
                if (DEBUG_PRINT)
                {
                    printf("::New address on interface %s\n", iface->name);
                    if_print_table(iface);
                    printf("\n");
                }
            }
            else
            {
                if_update_trans(arp_table, (unsigned char*)&a_hdr->ar_sha);
            }
            /* end packet info check*/

            /* check our queue now that our arp table has been added/updated */
            check_queue(sr,iface);

            /* Reply to requests sent to us*/
            if ((a_hdr->ar_op      == htons(ARP_REQUEST)) &&
                (a_hdr->ar_tip     = iface->ip ) )
                sr_arp_reply(sr,packet,len,interface);
            break;

        case ETHERTYPE_IP:
            if (DEBUG_PRINT)
                printf("\tIP::");

            sr_ip_reply(sr, packet, len, interface);
            break;
    
        default:
            if (DEBUG_PRINT)
                printf("\tUNKNOWN Ethernet type. Packet dropped.\n");
            break;
    }


}/* end sr_ForwardPacket */

/*-----------------------------------------------------------------------------
 * Method: sr_arp_reply()
 * Scope: local
 *
 * replies to ARP request packet with ARP reply using details found in senders packet
 *---------------------------------------------------------------------------*/
int sr_arp_reply(struct sr_instance* sr,
                           uint8_t * packet /* lent */,
                           unsigned int len,
                           char* interface  /* lent */)
{

    assert(interface);
    struct sr_if* iface = sr_get_interface(sr, interface);
    struct sr_ethernet_hdr* e_hdr = 0;  //recieved packet eth header
    struct sr_arphdr*       a_hdr = 0;  //recieved packet arp header

    uint8_t * reply_packet;             //Packet to reply to ARP with
    struct sr_ethernet_hdr* r_e_hdr = 0;//reply eth header
    struct sr_arphdr*       r_a_hdr = 0;//reply arp header

    assert(iface);
    assert(packet);

    e_hdr = (struct sr_ethernet_hdr*)packet;
    a_hdr = (struct sr_arphdr*)(packet + sizeof(struct sr_ethernet_hdr));

    reply_packet = (uint8_t*) malloc (sizeof(struct sr_ethernet_hdr) + sizeof(struct sr_arphdr));

    /*lolwut?*/
    if (reply_packet == NULL)
    {
        return -1;
    }

    r_e_hdr = (struct sr_ethernet_hdr*)reply_packet;
    r_a_hdr = (struct sr_arphdr*)(reply_packet + sizeof(struct sr_ethernet_hdr));

    memcpy(r_e_hdr->ether_dhost, e_hdr->ether_shost,6);
    memcpy(r_e_hdr->ether_shost, iface->addr,6);

    memcpy(r_a_hdr->ar_tha, a_hdr->ar_sha, 6);
    memcpy(r_a_hdr->ar_sha,iface->addr,6);

    r_e_hdr->ether_type = e_hdr->ether_type;
    r_a_hdr->ar_hln = a_hdr->ar_hln;
    r_a_hdr->ar_hrd = a_hdr->ar_hrd;
    r_a_hdr->ar_op = htons(ARP_REPLY);
    r_a_hdr->ar_pln = a_hdr->ar_pln;
    r_a_hdr->ar_pro = a_hdr->ar_pro;
    r_a_hdr->ar_sip =  iface->ip;
    r_a_hdr->ar_tip = a_hdr->ar_sip;

    sr_send_packet(sr,reply_packet,len,interface);

    free(reply_packet);
    return 0;
}/* -- sr_arp_reply -- */


int sr_arp_request(struct sr_instance* sr,
                            uint32_t* tar_ip,
                            char* interface )
{
    struct sr_if*           iface = sr_get_interface(sr, interface);
    uint8_t*                req_packet = 0;
    struct sr_ethernet_hdr* e_hdr = 0;
    struct sr_arphdr*       a_hdr = 0;
    char*                   broadcast = "\377\377\377\377\377\377";
    //uint32_t                 broadcast = 0xFFFF;
    
    assert(iface);

    
    req_packet = (uint8_t*) malloc (sizeof(struct sr_ethernet_hdr) + sizeof(struct sr_arphdr));
    assert(req_packet);

    bzero(req_packet, (sizeof(struct sr_ethernet_hdr) + sizeof(struct sr_arphdr)));

    e_hdr = (struct sr_ethernet_hdr*)req_packet;
    a_hdr = (struct sr_arphdr*)(req_packet + sizeof(struct sr_ethernet_hdr));

    //our IP address
    memcpy(e_hdr->ether_shost, iface->addr,6);
    memcpy(e_hdr->ether_dhost, broadcast, 6);

    //our hardware address
    memcpy(a_hdr->ar_sha, iface->addr,6);
    memcpy(a_hdr->ar_tha, broadcast, 6);

    e_hdr->ether_type = htons(ETHERTYPE_ARP);    
    a_hdr->ar_hrd = htons(1);           /*Format: Ethernet (10mb)*/
    a_hdr->ar_pro = htons(2048);        /*IPv4 address protocol format*/
    a_hdr->ar_hln = 6;           /*Ethernet and IEEE 802 MAC addr len = 6*/
    a_hdr->ar_op = htons(ARP_REQUEST);
    a_hdr->ar_pln = 4;           /*IPv4 addr len = 4 bytes*/
    a_hdr->ar_sip =  iface->ip;
    a_hdr->ar_tip = *tar_ip;
    sr_send_packet(sr,req_packet,(sizeof(struct sr_ethernet_hdr)+ sizeof(struct sr_arphdr)),interface);

    free(req_packet);
    return 0;
}

/*--------------------------------------------------------------------- 
 * Method: sr_ip_reply()
 * Scope: Local
 *---------------------------------------------------------------------*/
int  sr_ip_reply(
        struct sr_instance* sr,
        uint8_t * packet        /* packet from ethernet header */,
        unsigned int len        /* len of packet*/,
        char* interface)
{
    assert(sr);
    assert(packet);
    assert(len);
    assert(interface);

    struct sr_if*           iface = 0;      /* Packet's incoming interface */
    struct sr_if*           d_iface = 0;    /* Packet destination's interface */
    int                     arp_queue_status = 0;
    uint8_t *               ip_packet = packet + sizeof(struct sr_ethernet_hdr);
    struct ip*              ip_hdr = (struct ip*)ip_packet;
    struct ip*              r_ip_hdr = 0;
    uint8_t*                reply_packet = 0;
    uint8_t                 r_proto = ip_hdr->ip_p;
    struct sr_icmphdr*      icmp_hdr = 0;
    uint16_t                r_len = ntohs(ip_hdr->ip_len);

    struct sr_ethernet_hdr* e_hdr = (struct sr_ethernet_hdr*)packet;
    struct sr_ethernet_hdr* r_e_hdr = 0;
    uint8_t*                r_e_dhost = 0;
    struct sr_if*           r_iface = 0;    /* Reply packet's interfacer */
    struct if_tt*           arp_entry = 0;
    uint32_t                gateway = 0;

    /*Perform sanity checks*/
    if (ip_hdr->ip_v != 4)
    {
        if (DEBUG_PRINT)
            printf("\n\t\tIP version %d not supported. Packet dropped.\n", ip_hdr->ip_v);

        return EXIT_FAILURE;
    }
    uint16_t orig_sum = ip_hdr->ip_sum;
    ip_hdr->ip_sum = 0;
    uint16_t calc_sum = sr_calc_checksum(sizeof(struct ip), (uint16_t*)ip_hdr);
    ip_hdr->ip_sum = orig_sum;
    if (calc_sum != orig_sum)
    {
        if (DEBUG_PRINT)
            printf("\n\t\tIP Checksum failed. Packet dropped.\n");

        return EXIT_FAILURE;
    }

    iface = sr_get_interface(sr, interface);
    assert(iface);

    d_iface = sr_get_interface_by_ip(sr, (uint32_t)ip_hdr->ip_dst.s_addr);
    r_iface = get_interface_by_rt(sr, ip_hdr->ip_dst.s_addr, &gateway);
    if (!(d_iface || r_iface))
    {
        if (DEBUG_PRINT)
            printf("\n\t\tUnknown return interface for packet. Packet dropped.\n");

        return EXIT_FAILURE;
    }

    if (!d_iface && r_iface)
        arp_queue_status = check_queue_status(r_iface, &ip_hdr->ip_dst);

    /* if ARP queue is full, drop packet */
    if (arp_queue_status < 0)
    {
        if (DEBUG_PRINT)
            printf("ARP queue full. Packet dropped.\n");

        return EXIT_FAILURE;
    }
    /* if number of awaiting packets for a given IP has exceeded 5, drop packets */
    else if (arp_queue_status == 5)
    {
        if (DEBUG_PRINT)
            printf("Awaiting packets exceeds 5. Packet dropped.\n");

        drop_packets(r_iface, &ip_hdr->ip_dst);
        
        reply_packet = sr_icmp_unreachable_reply(ip_packet, len - sizeof(struct sr_ethernet_hdr), ICMP_UNREACH_HOST);
        assert(reply_packet);
        r_proto = IPPROTO_ICMP;
        r_len = sizeof(struct ip)*2 + sizeof(struct sr_icmphdr) + 8;
    }
    /* if packet is for an interface on the router*/
    else if (d_iface)
    {
        switch (ip_hdr->ip_p)
        {
            case IPPROTO_ICMP:
                if (DEBUG_PRINT)
                    printf("ICMP::");

                icmp_hdr = (struct sr_icmphdr*)(ip_packet + sizeof(struct ip));
                reply_packet = sr_icmp_reply((uint8_t*)icmp_hdr, len - sizeof(struct sr_ethernet_hdr) - sizeof(struct ip));
                break;

            case IPPROTO_TCP:
            case IPPROTO_UDP:
                if (DEBUG_PRINT)
                    printf("%s::ICMP_UNREACH_PORT\n", ip_hdr->ip_p == IPPROTO_TCP ? "TCP" : "UDP");

                reply_packet = sr_icmp_unreachable_reply(ip_packet, len - sizeof(struct sr_ethernet_hdr), ICMP_UNREACH_PORT);
                assert(reply_packet);
                r_proto = IPPROTO_ICMP;
                r_len = sizeof(struct ip)*2 + sizeof(struct sr_icmphdr) + 8;

                break;

            default:
                if (DEBUG_PRINT)
                    printf("Unknown IP protocol 0x%x\n", ip_hdr->ip_p);
                //drop packet
                break;
        }
    }
    /* if the packet has expired (TRACEROUTE case too) */
    else if (ip_hdr->ip_ttl == 1)
    {
        if (DEBUG_PRINT)
            printf("TTL EXCEEDED IN TRANSIT\n");

        reply_packet = sr_icmp_ttl_reply((uint8_t*)ip_packet, len - sizeof(struct sr_ethernet_hdr));
        assert(reply_packet);
        r_proto = IPPROTO_ICMP;
        r_len = sizeof(struct ip)*2 + sizeof(struct sr_icmphdr) + 8;
        d_iface = iface;
    }
    else
    {
        if (DEBUG_PRINT)
            printf("FORWARD\n");

        /* make forward packet, decrement TTL and recalc checksum */
        reply_packet = malloc(len);
        memcpy(reply_packet + sizeof(struct sr_ethernet_hdr), ip_packet, len - sizeof(struct sr_ethernet_hdr) );

        r_ip_hdr = (struct ip*)(reply_packet + sizeof(struct sr_ethernet_hdr));
        r_ip_hdr->ip_ttl = r_ip_hdr->ip_ttl - 1;

        r_ip_hdr->ip_sum = 0;
        r_ip_hdr->ip_sum = sr_calc_checksum(sizeof(struct ip), (uint16_t *)r_ip_hdr);
    }

    char src[INET_ADDRSTRLEN], dst[INET_ADDRSTRLEN];
    inet_ntop(AF_INET, (void *)&ip_hdr->ip_src.s_addr, src, INET_ADDRSTRLEN);
    inet_ntop(AF_INET, (void *)&ip_hdr->ip_dst.s_addr, dst, INET_ADDRSTRLEN);
    printf("\t\tip_src=%s\tip_dst=%s\tlen=%d\n", src, dst, len);

    if (DEBUG_PACKET)
    {
        printf("\t\tReceived ");
        print_packet(packet, len);
    }

    if (!reply_packet)
    {
        if (DEBUG_PRINT)
        printf("\t\tPacket dropped.\n");
        return EXIT_FAILURE;
    }


    /* Build IP header if not done yet */
    r_ip_hdr = (struct ip*)(reply_packet + sizeof(struct sr_ethernet_hdr));
    if (r_ip_hdr->ip_ttl == 0)
    {
        memcpy(r_ip_hdr, ip_hdr, sizeof(struct ip));

        r_ip_hdr->ip_dst = ip_hdr->ip_src;
        r_ip_hdr->ip_src.s_addr = d_iface->ip;
        r_ip_hdr->ip_len = htons(r_len);
        r_ip_hdr->ip_p = r_proto;               /* Protocol code */
        r_ip_hdr->ip_ttl =  IP_DEFAULT_TTL;

        r_ip_hdr->ip_sum = 0;
        r_ip_hdr->ip_sum = sr_calc_checksum(sizeof(struct ip), (uint16_t *)r_ip_hdr);
    }
    

    /* Build Ethernet header */
    r_e_hdr = (struct sr_ethernet_hdr*)reply_packet;
    if (d_iface)
    {
        r_e_dhost = e_hdr->ether_shost;
        r_iface = iface;
    }
    else if (r_iface)
    {
        arp_entry = if_find_trans(r_iface, &gateway);
        if (arp_entry)
            r_e_dhost = arp_entry->hrd_addr;
    }

    r_e_hdr->ether_type = e_hdr->ether_type;
    memcpy(r_e_hdr->ether_shost, r_iface->addr, ETHER_ADDR_LEN);


    if (r_e_dhost)
    {
        memcpy(r_e_hdr->ether_dhost, r_e_dhost, ETHER_ADDR_LEN);
        if (sr_send_packet(sr, reply_packet, sizeof(struct sr_ethernet_hdr) + r_len, r_iface->name) != 0 && DEBUG_PRINT)
        {
            printf("\t\tReply failed...\n");
        }
        else if (DEBUG_PACKET)
        {
            printf("\t\tSent ");
            print_packet(reply_packet, len);
        }
        free (reply_packet);
    }
    else
    {
        if (DEBUG_PRINT)
            printf("\t\tAwaiting MAC address.\n");

        queue_packet(r_iface, reply_packet, sizeof(struct sr_ethernet_hdr) + r_len);
        if (arp_queue_status == 0)
        {
            sr_arp_request(sr, &gateway, r_iface->name);
        }
    }

    return EXIT_SUCCESS;
} /* -- sr_ip_reply -- */

/*-----------------------------------------------------------------------------
 * Method: sr_icmp_reply()
 * Scope: Local
 *
 *---------------------------------------------------------------------------*/
uint8_t* sr_icmp_reply(
        uint8_t * icmp_packet/* packet from icmp header */,
        unsigned int len /* len of packet*/)
{
    struct sr_icmphdr* icmp_hdr = (struct sr_icmphdr*)icmp_packet;
    assert(icmp_hdr);
    uint8_t* reply_packet = 0;

    /* Sanity check */
    uint16_t orig_sum = icmp_hdr->icmp_sum;
    icmp_hdr->icmp_sum = 0;
    uint16_t calc_sum = sr_calc_checksum(len, (uint16_t*)icmp_packet);
    icmp_hdr->icmp_sum = orig_sum;
    if (calc_sum != orig_sum)
    {
        if (DEBUG_PRINT)
            printf("\n\t\tICMP Checksum failed. Packet dropped.\n");

        return reply_packet;
    }

    switch(icmp_hdr->icmp_type)
    {
        case ICMP_ECHO_REQUEST:
            if (DEBUG_PRINT)
                printf("ECHO_REQUEST\n");

            reply_packet = sr_icmp_echo_reply(icmp_packet, len);
            assert(reply_packet);
            break;

        default:
            if (DEBUG_PRINT)
                printf("\tOut of router's functionality ICMP_TYPE=%d\n", ntohs(icmp_hdr->icmp_type));

            break;
    }

    return reply_packet;
}/* -- sr_icmp_reply -- */

/*-----------------------------------------------------------------------------
 * Method: sr_icmp_echo_reply()
 * Scope: Local
 *
 *---------------------------------------------------------------------------*/
uint8_t* sr_icmp_echo_reply(
        uint8_t * icmp_packet/* packet from icmp header */,
        unsigned int len /* len of packet*/)
{
    struct sr_icmphdr* r_icmp_hdr = 0;

    uint8_t * reply_packet;

    reply_packet = malloc (sizeof(struct sr_ethernet_hdr) + sizeof(struct ip) + len);
    assert(reply_packet);
    bzero(reply_packet, sizeof(struct sr_ethernet_hdr) + sizeof(struct ip) + len);

    /* copy ICMP header info + data */
    r_icmp_hdr = (struct sr_icmphdr*)(reply_packet + sizeof(struct sr_ethernet_hdr) + sizeof(struct ip));
    memcpy(r_icmp_hdr, icmp_packet, len);
    
    /* change relevant ICMP fields */
    r_icmp_hdr->icmp_type = ICMP_ECHO_REPLY;     //0
    r_icmp_hdr->icmp_code = ICMP_ECHO_CODE;      //0

    /* calculate checksum over entire ICMP message */
    r_icmp_hdr->icmp_sum = 0;                                /* reset checksum */
    r_icmp_hdr->icmp_sum = sr_calc_checksum(len, (uint16_t *)r_icmp_hdr);

    return reply_packet;
} /* -- sr_icmp_echo_reply -- */


/*-----------------------------------------------------------------------------
 * Method: sr_icmp_unreachable_reply()
 * Scope: Local
 *
 *---------------------------------------------------------------------------*/
uint8_t * sr_icmp_unreachable_reply(
        uint8_t * ip_packet/* packet from ip header */,
        unsigned int len /* len of packet*/,
        uint8_t icmp_code)
{
    struct sr_icmphdr* r_icmp_hdr = 0;
    uint16_t data_len = sizeof(struct ip) + 8;
    uint16_t r_len;

    uint8_t * reply_packet;

    if (data_len > len)
        data_len = len;
    r_len = sizeof(struct sr_ethernet_hdr) + sizeof(struct ip) + sizeof(struct sr_icmphdr) + data_len;

    reply_packet = malloc (r_len);
    assert(reply_packet);
    bzero(reply_packet, r_len);

    r_icmp_hdr = (struct sr_icmphdr*)(reply_packet + sizeof(struct sr_ethernet_hdr) + sizeof(struct ip));

    /* change relevant ICMP fields */
    r_icmp_hdr->icmp_type = ICMP_UNREACH;
    r_icmp_hdr->icmp_code = icmp_code;

    /* copy IP header and 8B of data */
    memcpy((uint8_t *)r_icmp_hdr + sizeof(struct sr_icmphdr), ip_packet, data_len);

    /* calculate checksum over entire ICMP message */
    r_icmp_hdr->icmp_sum = 0;
    r_icmp_hdr->icmp_sum = sr_calc_checksum(sizeof(struct sr_icmphdr) + data_len, (uint16_t *)r_icmp_hdr);

    return reply_packet;
} /* -- sr_icmp_unreachable_reply -- */



/*-----------------------------------------------------------------------------
 * Method: sr_icmp_ttl_reply()
 * Scope: Local
 *
 *---------------------------------------------------------------------------*/
uint8_t* sr_icmp_ttl_reply(
        uint8_t * ip_packet/* packet from ip header */,
        unsigned int len /* len of packet*/)
{
    struct sr_icmphdr* r_icmp_hdr = 0;
    uint16_t r_len;
    uint16_t data_len = sizeof(struct ip) + 8;
    uint8_t * reply_packet;

    r_len = sizeof(struct sr_ethernet_hdr) + sizeof(struct ip) + sizeof(struct sr_icmphdr) + data_len;

    reply_packet = malloc (r_len);
    assert(reply_packet);
    bzero(reply_packet, r_len);

    r_icmp_hdr = (struct sr_icmphdr*)(reply_packet + sizeof(struct sr_ethernet_hdr) + sizeof(struct ip));

    /* set nonzero ICMP fields */
    r_icmp_hdr->icmp_type = ICMP_TTL_EXCEED;
    r_icmp_hdr->icmp_code = ICMP_TTL_EX_TRANS;

    /* copy IP datagram */
    memcpy((uint8_t *)r_icmp_hdr + sizeof(struct sr_icmphdr), ip_packet, data_len);

    /* calculate checksum over entire ICMP message */
    r_icmp_hdr->icmp_sum = 0;
    r_icmp_hdr->icmp_sum = sr_calc_checksum(sizeof(struct sr_icmphdr) + data_len, (uint16_t *)r_icmp_hdr);

    return reply_packet;
} /* -- sr_icmp_ttl_reply -- */



/*-----------------------------------------------------------------------------
 * Method: sr_calc_checksum()
 * Scope: Global
 *
 * Source: RFC 1071
 * Source: http://stackoverflow.com/questions/1962746/how-do-i-compute-an-rfc-791-ip-header-checksum
 *
 *---------------------------------------------------------------------------*/
uint16_t sr_calc_checksum(uint16_t len, uint16_t * packet)
{
    uint32_t checksum = 0;

    while (len > 1) {
        checksum += *packet++;
        len -= 2;
    }

    // Add left-over byte, if any
    if (len > 0)
        checksum += * (unsigned char *) packet;

    while (checksum >> 16)
        checksum = (checksum & 0xFFFF) + (checksum >> 16);

	checksum = ~checksum;
    return ((uint16_t)checksum);
}